package com.testcolision.simulation;

import com.badlogic.gdx.math.Vector2;

public class Vehicle extends RigidBody {
	private Wheel[] wheels = new Wheel[4];

	@Override
	public void Setup(Vector2 halfSize, float mass) {
		// front wheels
		wheels[0] = new Wheel(new Vector2(halfSize.x, halfSize.y), 0.5f);
		wheels[1] = new Wheel(new Vector2(-halfSize.x, halfSize.y), 0.5f);

		// rear wheels
		wheels[2] = new Wheel(new Vector2(halfSize.x, -halfSize.y), 0.5f);
		wheels[3] = new Wheel(new Vector2(-halfSize.x, -halfSize.y), 0.5f);

		super.Setup(halfSize, mass);
	}

	float steeringLock = 0.75f;

	public void SetSteering(float steering) {

		// apply steering angle to front wheels
		wheels[0].SetSteeringAngle(-steering * steeringLock);
		wheels[1].SetSteeringAngle(-steering * steeringLock);
	}

	float torque = 40.0f;

	public void SetThrottle(float throttle, boolean allWheel) {

		// apply transmission torque to back wheels
		if (allWheel) {
			wheels[0].AddTransmissionTorque(throttle * torque);
			wheels[1].AddTransmissionTorque(throttle * torque);
		}

		wheels[2].AddTransmissionTorque(throttle * torque);
		wheels[3].AddTransmissionTorque(throttle * torque);
	}

	float brakeTorque = 4.0f;

	public void SetBrakes(float brakes) {

		// apply brake torque apposing wheel vel
		for (Wheel wheel : wheels) {
			float wheelVel = wheel.GetWheelSpeed();
			wheel.AddTransmissionTorque(-wheelVel * brakeTorque * brakes);
		}
	}

	@Override
	public void Update(float timeStep) {
		for (Wheel wheel : wheels) {
			Vector2 worldWheelOffset = super.RelativeToWorld(wheel.GetAttachPoint());
			Vector2 worldGroundVel = super.PointVel(worldWheelOffset);
			Vector2 relativeGroundSpeed = super.WorldToRelative(worldGroundVel);
			Vector2 relativeResponseForce = wheel.CalculateForce(relativeGroundSpeed, timeStep);
			Vector2 worldResponseForce = super.RelativeToWorld(relativeResponseForce);

			super.AddForce(worldResponseForce, worldWheelOffset);
		}

		super.Update(timeStep);
	}
}
